package Estrategia_Mochila;

import lejos.hardware.Button;
import lejos.hardware.Sound;
import lejos.hardware.lcd.LCD;
import lejos.hardware.port.MotorPort;
import lejos.hardware.port.Port;
import lejos.hardware.port.SensorPort;
import lejos.utility.Delay;

public class Principal_Mochila {

	// Definicao das portas de conexao
	private static final Port SENSOR_COR = SensorPort.S2;
	private static final Port SENSOR_TOQUE_ESQ = SensorPort.S4;
	private static final Port SENSOR_TOQUE_DIR = SensorPort.S3;

	private static final Port MOTOR_TACO = MotorPort.D;
	private static final Port MOTOR_PA = MotorPort.C;
	private static final Port MOTOR_RODA_ESQ = MotorPort.A;
	private static final Port MOTOR_RODA_DIREITA = MotorPort.B;

	private static Thread subLancamento;
	private static Thread subNavegacao;

	private static Lancamento_Mochila lancamento;

	private static Navegacao_Mochila navegacao;
	private static boolean verifica;

	public static void main(String[] args) {
		Sound.beep();
		LCD.drawString("Ajuste o taco na posicao central zero", 1, 1);
		LCD.drawString("Levante a pa", 1, 2);

		Button.waitForAnyPress();
		init();

		subLancamento.start();
		subNavegacao.start();

		while (true) {
			verifica = true;
			lancamento.setVelocidadeBatida(300);
			lancamento.setAnguloBatida(90);
			lancamento.setAnguloRetorno(30);

			if (navegacao.encontrouMochila) {
				while (verifica) {
					Delay.msDelay(2000);
					if (!lancamento.getBolaLancada()
							&& lancamento.getTemBolinha()) {
						if (lancamento.getBolaAzul()) {
							lancamento
									.lancarBolaAzul(lancamento.getPosicaoTaco() == lancamento.ESQUERDA ? lancamento.DIREITA
											: lancamento.ESQUERDA);
						} else {
							lancamento.lancarBolaLaranja();
						}
					} else {
						verifica = false;
						navegacao.encontrouMochila = false;
					}
				}
			} else {
				if (!lancamento.getBolaLancada() && lancamento.getTemBolinha()) {
					if (lancamento.getBolaAzul()) {
						lancamento
								.lancarBolaAzul(lancamento.getPosicaoTaco() == lancamento.ESQUERDA ? lancamento.DIREITA
										: lancamento.ESQUERDA);
					}
				}
			}

			if (Button.ESCAPE.isDown()) {
				lancamento.fim = true;
				navegacao.fim = true;
				break;
			}
		}
	}

	private static void init() {
		verifica = true;
		lancamento = new Lancamento_Mochila(SENSOR_COR, MOTOR_TACO);
		navegacao = new Navegacao_Mochila(SENSOR_TOQUE_ESQ, SENSOR_TOQUE_DIR,
				MOTOR_RODA_ESQ, MOTOR_RODA_DIREITA, MOTOR_PA);

		subLancamento = new Thread(lancamento, "Subsistema de lancamento");
		subNavegacao = new Thread(navegacao, "Subsistema de navegacao");

	}
}